/******************************************************************
 * 作者：sipeiliu
 * 邮箱：sipeiliu@163.com
 * 描述：距离控制，障碍物检测，卡尔曼滤波算法
 * 日期：2023.4.20
 ******************************************************************/

#include "/opt/sighter/inc/ultrasonic.h"
#include "/opt/sighter/inc/distance.h"
#include "/opt/sighter/inc/motor.h"

#include <unistd.h>

/*卡尔曼滤波算法*/
void kalman_filter(void)
{
    /*初始化参数*/
    const int err_mea = 3;                               // 测量误差,即传感器精度3mm
    int dis_est = global_distance - global_speed * 1000; // 估计1s后距离=此刻距离-速度*间隔时间1s
    int err_est = dis_est * 0.05;                        // 估计误差，单位mm
    int dis_mea;                                         // 测量距离, 单位mm
    int kal_gain;                                        // 卡尔曼增益

    sleep(1); // 间隔1s
    /*迭代10次*/
    for (int i = 0; i < 10; i++)
    {
        /*测量1s后距离*/
        dis_mea = global_distance;
        /*计算卡尔曼增益*/
        kal_gain = err_est / (err_est + err_mea);
        /*计算估计值*/
        dis_est = dis_est + kal_gain * (dis_mea - dis_est);
        /*更新估计误差*/
        err_est = (1 - kal_gain) * err_est;
    }

    /*即将发送碰撞处理*/
    if (dis_est < 100)
    {
        motor_run(STOP);      // 停车
        motor_run(CLOCKWISE); // 转动
        motor_run(STOP);      // 停车
    }
}

/*控制距离，通过超声波传感器检测障碍物*/
void *thread_distance(void *arg)
{
    enable_ultrasonic(); // 使能超声波传感器
    while (1)            // 使能卡尔曼算法
    {
        void kalman_filter();
    }
}